#include "rikibot_foc_driver/rikibot_driver.hpp"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>  // 添加这行
#include <iostream>
#include <cmath>
#include <iomanip>
#include <cstring>

RikibotDriver::RikibotDriver(ros::NodeHandle &nh) 
    : nh_(nh), 
      x_(0.0), y_(0.0), theta_(0.0),
      wheel_diameter_(0.165), 
      wheel_distance_(0.33),
      buffer_pos_(0)
{
    // 参数初始化
    nh_.param<std::string>("port", port_, "/dev/ttyUSB0");
    nh_.param("baudrate", baudrate_, 115200);
    nh_.param<std::string>("base_frame_id", base_frame_id_, "base_footprint");
    nh_.param<std::string>("odom_frame_id", odom_frame_id_, "odom");
    nh_.param("publish_tf", publish_tf_, true);
    nh_.param("wheel_diameter", wheel_diameter_, 0.165);
    nh_.param("wheel_distance", wheel_distance_, 0.33);
    nh_.param("swap_bytes", swap_bytes_, false);
    
    // 设置日志级别
    std::string log_level;
    nh_.param<std::string>("log_level", log_level, "info");
    if (log_level == "debug") {
        if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) {
            ros::console::notifyLoggerLevelsChanged();
        }
    }
    
    wheel_circumference_ = M_PI * wheel_diameter_;

    // 初始化串口
    try {
        serial_.setPort(port_);
        serial_.setBaudrate(baudrate_);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        serial_.setTimeout(to);
        serial_.setBytesize(serial::eightbits);
        serial_.setStopbits(serial::stopbits_one);
        serial_.setParity(serial::parity_none);
        serial_.setFlowcontrol(serial::flowcontrol_none);
        serial_.open();
        
        ROS_INFO("Serial port opened: %s", port_.c_str());
        ROS_INFO("Baudrate: %d", baudrate_);
        ROS_INFO("Byte swap: %s", swap_bytes_ ? "enabled" : "disabled");
    } catch (serial::IOException& e) {
        ROS_FATAL("Failed to open serial port %s: %s", port_.c_str(), e.what());
        ros::shutdown();
    } catch (serial::SerialException& e) {
        ROS_FATAL("Serial exception: %s", e.what());
        ros::shutdown();
    }

    // 创建全局节点句柄
    ros::NodeHandle global_nh;
    
    // 使用全局命名空间创建发布者
    odom_pub_ = global_nh.advertise<nav_msgs::Odometry>("odom", 50);
    battery_pub_ = global_nh.advertise<sensor_msgs::BatteryState>("battery", 10);
    
    // 订阅器使用私有命名空间
    cmd_vel_sub_ = global_nh.subscribe<geometry_msgs::Twist>("cmd_vel", 10, &RikibotDriver::cmdVelCallback, this);

    last_time_ = ros::Time::now();
}

uint16_t RikibotDriver::ntohs(uint16_t value) {
    if (swap_bytes_) {
        return (value >> 8) | (value << 8);
    }
    return value;
}

void RikibotDriver::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg) {
    // 将Twist消息转换为电机转速 (m/s -> RPM)
    double linear = msg->linear.x;
    double angular = msg->angular.z;
    
    // 差分驱动运动学模型
    double left_speed = linear - angular * wheel_distance_ / 2.0;
    double right_speed = linear + angular * wheel_distance_ / 2.0;
    
    // 转换为RPM (转/分钟)
    double left_rpm = (left_speed * 60.0) / wheel_circumference_;
    double right_rpm = (right_speed * 60.0) / wheel_circumference_;
    
    // 构建命令包
    RikibotCommand cmd;
    cmd.start = START_FRAME;
    // 由于电机对立安装，右电机需要反转
    cmd.mSpeedL = static_cast<int16_t>(left_rpm);
    cmd.mSpeedR = static_cast<int16_t>(-right_rpm);
    cmd.checksum = calculateCommandChecksum(cmd);
    
    // 发送命令
    try {
        ROS_DEBUG("Sending command: L=%d RPM, R=%d RPM", cmd.mSpeedL, cmd.mSpeedR);
        serial_.write(reinterpret_cast<const uint8_t*>(&cmd), sizeof(cmd));
    } catch (serial::IOException& e) {
        ROS_ERROR("Serial write error: %s", e.what());
    } catch (serial::PortNotOpenedException& e) {
        ROS_ERROR("Serial port not open: %s", e.what());
    }
}

void RikibotDriver::readSerial() {
    if (!serial_.isOpen()) {
        ROS_WARN_THROTTLE(5, "Serial port not open");
        return;
    }
    
    // 限制每次读取的最大数据量
    size_t max_read = std::min(serial_.available(), sizeof(serial_buffer_) - buffer_pos_);
    if (max_read == 0) {
        if (buffer_pos_ >= sizeof(serial_buffer_)) {
            ROS_WARN("Serial buffer full, resetting");
            buffer_pos_ = 0;
        }
        return;
    }
    
    // 读取数据
    size_t bytes_read = serial_.read(serial_buffer_ + buffer_pos_, max_read);
    buffer_pos_ += bytes_read;
    
    // 添加帧间隔时间检查
    static ros::Time last_valid_frame = ros::Time::now();
    if ((ros::Time::now() - last_valid_frame).toSec() > 0.1) {
        // 超过100ms没有有效帧，重置缓冲区
        buffer_pos_ = 0;
        ROS_DEBUG("Resetting buffer due to timeout");
    }
    
    // 处理缓冲区中的数据
    size_t processed = 0;
    while (buffer_pos_ - processed >= sizeof(RikibotFeedback)) {
        uint8_t* current = serial_buffer_ + processed;
        RikibotFeedback* fb = reinterpret_cast<RikibotFeedback*>(current);
        
        // 检查起始帧
        uint16_t start_frame = ntohs(fb->start);
        if (start_frame != START_FRAME) {
            // 起始帧不匹配，移动一个字节
            processed++;
            continue;
        }
        
        // 检查校验和
        uint16_t received_checksum = ntohs(fb->checksum);
        uint16_t calculated_checksum = calculateChecksum(*fb);
        
        if (received_checksum != calculated_checksum) {
            // 添加统计信息
            static int error_count = 0;
            static int total_count = 0;
            error_count++;
            total_count++;
            
            // 每10个错误报告一次
            if (error_count % 10 == 0) {
                ROS_WARN("Checksum error rate: %.2f%% (%d/%d)", 
                         (error_count * 100.0) / total_count,
                         error_count, total_count);
            }
            
            // 打印原始帧用于调试
            ROS_DEBUG("Checksum mismatch: received 0x%04X, calculated 0x%04X", 
                     received_checksum, calculated_checksum);
            
            std::stringstream ss;
            ss << "Raw frame: ";
            for (size_t i = 0; i < sizeof(RikibotFeedback); i++) {
                ss << std::hex << std::setw(2) << std::setfill('0') 
                   << static_cast<int>(current[i]) << " ";
            }
            ROS_DEBUG("%s", ss.str().c_str());
            
            processed++;
            continue;
        }
        
        // 处理有效帧
        processFeedback(*fb);
        last_valid_frame = ros::Time::now();
        
        // 移动到下一帧
        processed += sizeof(RikibotFeedback);
    }
    
    // 移除已处理的数据
    if (processed > 0) {
        size_t remaining = buffer_pos_ - processed;
        if (remaining > 0) {
            memmove(serial_buffer_, serial_buffer_ + processed, remaining);
        }
        buffer_pos_ = remaining;
    }
}

void RikibotDriver::processFeedback(const RikibotFeedback &fb) {
    ros::Time current_time = ros::Time::now();
    double dt = (current_time - last_time_).toSec();
    if (dt <= 0) dt = 0.01;  // 防止零或负时间间隔
    last_time_ = current_time;
    
    // 应用字节序转换
    int16_t rpmL = ntohs(fb.rpmL);
    int16_t rpmR = ntohs(fb.rpmR);
    
    // 计算线速度 (m/s)
    double left_speed = (rpmL * wheel_circumference_) / 60.0;
    double right_speed = (rpmR * wheel_circumference_) / 60.0;
    
    // 由于右电机对立安装，实际速度取反
    right_speed = -right_speed;
    
    // 计算线速度和角速度
    double linear = (left_speed + right_speed) / 2.0;
    double angular = (right_speed - left_speed) / wheel_distance_;
    
    // 更新位姿 (欧拉积分)
    double delta_x = linear * cos(theta_) * dt;
    double delta_y = linear * sin(theta_) * dt;
    double delta_theta = angular * dt;
    
    x_ += delta_x;
    y_ += delta_y;
    theta_ += delta_theta;
    
    // 角度标准化到 [-π, π]
    theta_ = atan2(sin(theta_), cos(theta_));
    
    ROS_DEBUG("Feedback: L=%d RPM, R=%d RPM, Linear=%.3f m/s, Angular=%.3f rad/s", 
              rpmL, rpmR, linear, angular);
    
    // 创建并发布里程计消息
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = odom_frame_id_;
    odom.child_frame_id = base_frame_id_;
    
    // 位置
    odom.pose.pose.position.x = x_;
    odom.pose.pose.position.y = y_;
    odom.pose.pose.position.z = 0.0;

    tf2::Quaternion q;
    q.setRPY(0, 0, theta_);
    odom.pose.pose.orientation = tf2::toMsg(q);  // 直接使用 tf2::toMsg
  
    // 设置协方差 (根据实际情况调整)
    odom.pose.covariance[0] = 0.01;  // x
    odom.pose.covariance[7] = 0.01;  // y
    odom.pose.covariance[35] = 0.03; // yaw
    
    // 速度
    odom.twist.twist.linear.x = linear;
    odom.twist.twist.angular.z = angular;
    
    odom.twist.covariance[0] = 0.02;  // x速度
    odom.twist.covariance[7] = 0.02;  // y速度
    odom.twist.covariance[35] = 0.04; // 角速度
    
    odom_pub_.publish(odom);
    
    // 发布TF变换
    if (publish_tf_) {
        geometry_msgs::TransformStamped transformStamped;
        transformStamped.header.stamp = current_time;
        transformStamped.header.frame_id = odom_frame_id_;
        transformStamped.child_frame_id = base_frame_id_;
        transformStamped.transform.translation.x = x_;
        transformStamped.transform.translation.y = y_;
        transformStamped.transform.translation.z = 0.0;
        
        transformStamped.transform.rotation = tf2::toMsg(q);
        
        tf_broadcaster_.sendTransform(transformStamped);
    }
    
    // 发布电池状态
    sensor_msgs::BatteryState battery;
    battery.header.stamp = current_time;
    battery.header.frame_id = base_frame_id_;
    battery.voltage = static_cast<float>(ntohs(fb.batVoltage)) / 10.0f;  // 假设单位是0.1V
    battery.percentage = std::nanf("");  // 未知百分比
    battery.present = true;
    
    battery_pub_.publish(battery);
}

uint16_t RikibotDriver::calculateChecksum(const RikibotFeedback &fb) {
    // 注意：这里使用原始数据计算校验和（不进行字节序转换）
    uint16_t sum = fb.start;
    sum ^= fb.rpmR;
    sum ^= fb.rpmL;
    sum ^= fb.batVoltage;
    sum ^= fb.boardTemp;
    sum ^= fb.curL_DC;
    sum ^= fb.curR_DC;
    return sum;
}

uint16_t RikibotDriver::calculateCommandChecksum(const RikibotCommand &cmd) {
    return cmd.start ^ cmd.mSpeedR ^ cmd.mSpeedL;
}

void RikibotDriver::run() {
    ros::Rate rate(50);  // 50Hz
    while (ros::ok()) {
        readSerial();
        ros::spinOnce();
        rate.sleep();
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "rikibot_driver");
    ros::NodeHandle nh("~");
    
    RikibotDriver driver(nh);
    driver.run();
    
    return 0;
}